#include <rtthread.h>
#include "./Avoid.h"
#include "./Modbus/RTU/Master/modbusrtu_master.h"

static void main_thread(void *parameter);

static struct rt_thread _thread;
ALIGN(RT_ALIGN_SIZE)
static char _thread_stack[512];

static enum Sensor_Enum avoid[4]; /* 0~3对应前后左右4个避障 */
static unsigned short avoid_temp[4];

static struct rt_event _event;
static unsigned short pbs_area;
static char slave_addr;

void Avoid_Init(char addr)
{
    rt_err_t rt_result;
    slave_addr = addr;
    rt_event_init(&_event, "avoid event", RT_IPC_FLAG_PRIO);
    rt_result = rt_thread_init(&_thread, "avoid thread", main_thread, RT_NULL, _thread_stack, sizeof(_thread_stack), 5, 2);
    if (rt_result == RT_EOK)
    {
        rt_thread_startup(&_thread); /* 启动线程 */
    }
    else
    {
        rt_kprintf("avoid thread error\n");
    }
}

enum Sensor_Enum Avoid_Read(int index)
{
    index %= 4;
    return avoid[index];
}

void main_thread(void *parameter)
{
    rt_err_t rt_result;
    rt_uint32_t event_value;
    Modbus_Error_Code mb_result;
    while (1)
    {
        rt_result = rt_event_recv(&_event, 1 << 0, RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_OR, 10, &event_value);
        if (RT_EOK == rt_result)
        {
            mb_result = MR_Writer_Holding_Register(slave_addr, 0x0013, pbs_area, 100); /* 设置PBS区域 */
            if (Modbus_Master_OK != mb_result)
            {
                rt_event_send(&_event, 1 << 0);
            }
        }
        MR_Read_Holding_Registers(slave_addr, 0x0000, 4, avoid_temp, RT_WAITING_FOREVER);
        unsigned short temp = 0;
        for (int i = 0; i < 4; i++)
        {
            avoid[i] = (enum Sensor_Enum)avoid_temp[i];
            temp += avoid_temp[i];
        }
    }
}

void Avoid_Set_Area(char area)
{
    pbs_area = area;
    rt_event_send(&_event, 1 << 0);
}
